/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2020 Shivang Patel
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of Willow Garage, Inc. nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
* Author: Shivang Patel
*
* Reference tutorial:
* https://navigation.ros.org/tutorials/docs/writing_new_nav2planner_plugin.html
*********************************************************************/

#ifndef NAV2_STRAIGHTLINE_PLANNER__STRAIGHT_LINE_PLANNER_HPP_
#define NAV2_STRAIGHTLINE_PLANNER__STRAIGHT_LINE_PLANNER_HPP_

#include <string>
#include <memory>
#include <list>
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/point.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include <geometry_msgs/msg/pose2_d.hpp>
#include <geometry_msgs/msg/polygon.hpp>
#include "cv_bridge/cv_bridge/cv_bridge.h"
#include "nav2_core/global_planner.hpp"
#include "nav_msgs/msg/path.hpp"
#include "nav2_util/robot_utils.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "nav_msgs/srv/get_map.hpp"
#include "nav2_msgs/srv/get_map_nav2.hpp"
#include <ipa_building_msgs/action/room_exploration.hpp>
#include "ipa_building_navigation/concorde_TSP.h"
#include "ipa_building_navigation/tsp_solver_defines.h"
#include "ipa_building_navigation/A_star_pathplanner.h"
#include "ipa_room_exploration/voronoi.hpp"
#include "ipa_room_exploration/room_rotator.h"
#include "ipa_room_exploration/map_accessibility_analysis.h"
#include "ipa_room_exploration/boustrophedon_explorator.h"
#include "ipa_room_exploration/neural_network_explorator.h"
#include "ipa_room_exploration/convex_sensor_placement_explorator.h"
#include "ipa_room_exploration/flow_network_explorator.h"
#include "ipa_room_exploration/energy_functional_explorator.h"
#include "ipa_room_exploration/grid_point_explorator.h"

// #define DEBUG_PLOT

#ifndef dabs
#define dabs(a)     ((a) >= 0 ? (a):-(a))
#endif
#ifndef dmin
#define dmin(a, b)   ((a) <= (b) ? (a):(b))
#endif
#ifndef dmax
#define dmax(a, b)   ((a) >= (b) ? (a):(b))
#endif
#ifndef clamp
#define clamp(a, lower, upper)    dmax(dmin(a, upper), lower)
#endif

namespace ipa_coverage_planning_plugin
{
enum class MapMode
{
    /**
     * Together with associated threshold values (occupied and free):
     *   lightness >= occupied threshold - Occupied (100)
     *             ... (anything in between) - Unknown (-1)
     *    lightness <= free threshold - Free (0)
     */
    Trinary,
    /**
     * Together with associated threshold values (occupied and free):
     *   alpha < 1.0 - Unknown (-1)
     *   lightness >= occ_th - Occupied (100)
     *             ... (linearly interpolate to)
     *   lightness <= free_th - Free (0)
     */
    Scale,
    /**
     * Lightness = 0 - Free (0)
     *          ... (linearly interpolate to)
     * Lightness = 100 - Occupied (100)
     * Lightness >= 101 - Unknown
     */
    Raw,
};

struct LoadParameters
{
    std::string image_file_name;
    double resolution{0};
    std::vector<double> origin{0, 0, 0};
    double free_thresh;
    double occupied_thresh;
    MapMode mode;
    bool negate;
};

/* Map output part */

struct SaveParameters
{
    std::string map_file_name{""};
    std::string image_format{""};
    double free_thresh{0.0};
    double occupied_thresh{0.0};
    MapMode mode{MapMode::Trinary};
};

class IpaFullCoveragePath : public nav2_core::GlobalPlanner
{
public:
    IpaFullCoveragePath();

    ~IpaFullCoveragePath();

    // plugin configure
    void configure(
            const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent,
            std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
            std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;

    // plugin cleanup
    void cleanup() override;

    // plugin activate
    void activate() override;

    // plugin deactivate
    void deactivate() override;

    // This method creates path for given start and goal pose.
    nav_msgs::msg::Path createPlan(
            const geometry_msgs::msg::PoseStamped &start,
            const geometry_msgs::msg::PoseStamped &goal) override;

// Function that provides the functionality that a given field of view (fov) path gets mapped to a robot path by using the given parameters.
// To do so simply a vector operation is applied. If the computed robot pose is not in the free space, another accessible
// point is generated by finding it on the radius around the fov middlepoint s.t. the distance to the last robot position
// is minimized.
// Important: the room map needs to be an unsigned char single channel image, if inaccessible areas should be excluded, provide the inflated map
// robot_to_fov_vector in [m]
// returns robot_path in [m,m,rad]
    void mapPath(const cv::Mat &room_map, std::vector<geometry_msgs::msg::Pose2D> &robot_path,
                 const std::vector<geometry_msgs::msg::Pose2D> &fov_path,
                 const Eigen::Matrix<float, 2, 1> &robot_to_fov_vector,
                 const double map_resolution, const cv::Point2d map_origin, const cv::Point &starting_point);

    bool removeUnconnectedRoomParts(cv::Mat &room_map);

    void downsampleTrajectory(const std::vector<geometry_msgs::msg::Pose2D> &path_uncleaned,
                              std::vector<geometry_msgs::msg::Pose2D> &path, const double min_dist_squared);

    cv::Mat fromPathGetMat(std::string image_path);

    void matToMap(nav_msgs::msg::OccupancyGrid &map, const cv::Mat &mat);

    void mapToMat(const nav_msgs::msg::OccupancyGrid &map, cv::Mat &mat);

public:
    enum PlanningMode
    {
        PLAN_FOR_FOOTPRINT = 1, PLAN_FOR_FOV = 2
    };
    BoustrophedonExplorer boustrophedon_explorer_; // object that uses the boustrophedon exploration method to plan a path trough the room
    NeuralNetworkExplorator neural_network_explorator_; // object that uses the neural network method to create an exploration path
    convexSPPExplorator convex_SPP_explorator_; // object that uses the convex spp exploration methd to create an exploration path
    FlowNetworkExplorator flow_network_explorator_; // object that uses the flow network exploration method to create an exploration path
    EnergyFunctionalExplorator energy_functional_explorator_; // object that uses the energy functional exploration method to create an exploration path
    BoustrophedonVariantExplorer boustrophedon_variant_explorer_; // object that uses the boustrophedon variant exploration method to plan a path trough the room
    GridPointExplorator grid_point_planner; // object that uses the grid point method to plan a path trough a room

    double free_thresh;
    double occupied_thresh;
    double coverage_radius;
    std::vector<geometry_msgs::msg::Point32> fov_points;
    int planning_mode_;
    int room_exploration_algorithm_;
    bool display_trajectory_;
    int map_correction_closing_neighborhood_size_;
    bool return_path_;                // boolean used to determine if the server should return the computed coverage path in the  message
    bool execute_path_;                // boolean used to determine whether the server should navigate the robot along the computed coverage path
    double goal_eps_;                // distance between the published navigation goal and the robot to publish the next
    // navigation goal in the path
    bool use_dyn_goal_eps_;        // using a dynamic goal distance criterion: the larger the path's curvature, the more accurate the navigation
    bool interrupt_navigation_publishing_;    // variable that interrupts the publishing of navigation goals as long as needed, e.g. when during the execution
    // of the coverage path a trash bin is found and one wants to empty it directly and resume the path later.
    bool revisit_areas_;            // variable that turns functionality on/off to revisit areas that haven't been seen during the
    // execution of the coverage path, due to uncertainties or dynamic obstacles
    double left_sections_min_area_; // variable to determine the minimal area that not seen sections must have before they
    // are revisited after one run through the room
    std::string global_costmap_topic_;    // name of the global costmap topic
    std::string coverage_check_service_name_;    // name of the service to call for a coverage check of the driven trajectory
    std::string map_frame_;            // string that carries the name of the map frame, used for tracking of the robot
    std::string camera_frame_;                // string that carries the name of the camera frame, that is in the same kinematic chain as the map_frame and shows the camera pose

    // parameters specific to the grid point explorator
    int tsp_solver_;    // indicates which TSP solver should be used
    //   1 = Nearest Neighbor
    //   2 = Genetic solver
    //   3 = Concorde solver
    int64_t tsp_solver_timeout_;    // a sophisticated solver like Concorde or Genetic can be interrupted if it does not find a solution within this time, in [s], and then falls back to the nearest neighbor solver

    // parameters specific for the boustrophedon explorator
    double min_cell_area_;            // minimal area a cell can have, when using the boustrophedon explorator
    double path_eps_;        // the distance between points when generating a path
    double grid_obstacle_offset_;    // in [m], the additional offset of the grid to obstacles, i.e. allows to displace the grid by more than the standard half_grid_size from obstacles
    int max_deviation_from_track_;    // in [pixel], maximal allowed shift off the ideal boustrophedon track to both sides for avoiding obstacles on track
    // setting max_deviation_from_track=grid_spacing is usually a good choice
    // for negative values (e.g. max_deviation_from_track: -1) max_deviation_from_track is automatically set to grid_spacing
    int cell_visiting_order_;        // cell visiting order
    //   1 = optimal visiting order of the cells determined as TSP problem
    //   2 = alternative ordering from left to right (measured on y-coordinates of the cells), visits the cells in a more obvious fashion to the human observer (though it is not optimal)
    // parameters specific for the neural network explorator, see "A Neural Network Approach to Complete Coverage Path Planning" from Simon X. Yang and Chaomin Luo
    double step_size_; // step size for integrating the state dynamics
    int A_; // decaying parameter that pulls the activity of a neuron closer to zero, larger value means faster decreasing
    int B_; // increasing parameter that tries to increase the activity of a neuron when it's not too big already, higher value means a higher desired value and a faster increasing at the beginning
    int D_; // decreasing parameter when the neuron is labeled as obstacle, higher value means faster decreasing
    int E_; // external input parameter of one neuron that is used in the dynamics corresponding to if it is an obstacle or uncleaned/cleaned, E>>B
    double mu_; // parameter to set the importance of the states of neighboring neurons to the dynamics, higher value means higher influence
    double delta_theta_weight_; // parameter to set the importance of the traveleing direction from the previous step and the next step, a higher value means that the robot should turn less

    // parameters specific for the convexSPP explorator
    int cell_size_;                // size of one cell that is used to discretize the free space
    double delta_theta_;            // sampling angle when creating possible sensing poses in the convexSPP explorator

    // parameters specific for the flowNetwork explorator
    double curvature_factor_; // double that shows the factor, an arc can be longer than a straight arc when using the flowNetwork explorator
    double max_distance_factor_; // double that shows how much an arc can be longer than the maximal distance of the room, which is determined by the min/max coordinates that are set in the goal
    double delta_theta_two_yaw;
private:
    // TF buffer
    std::shared_ptr<tf2_ros::Buffer> tf_;
    // node ptr
    nav2_util::LifecycleNode::SharedPtr node_handle_;
    // Global Costmap
    nav2_costmap_2d::Costmap2D *costmap_;
    // The global frame of the costmap
    std::string global_frame_, name_;
    double interpolation_resolution_;
    float robotRadius;
    float toolRadius;
    float tile_size_;
    //rclcpp::Client<nav_msgs::srv::GetMap>::SharedPtr cpp_grid_client_;
    rclcpp::Client<nav2_msgs::srv::GetMapNav2>::SharedPtr cpp_grid_client_;
    geometry_msgs::msg::PoseStamped previous_goal_;
};

}  // namespace ipa_coverage_planning_plugin

#endif  // NAV2_STRAIGHTLINE_PLANNER__STRAIGHT_LINE_PLANNER_HPP_
